import json
import logging
import numpy as np
from src.common.processedMatrix import ProcessedMatrix
from bins.vehicleParams import VehicleParams
import threading
from src.common.processedmeasurement import ProcessedMeasurement

logger = logging.getLogger(__name__)


class MPCController:
    def __init__(self):

        self.longitudinal = None
        self.lateral = None
        self.Vehicle = None
        self.predicted_step = 25
        self.predicted_v = None
        self.dt = 0.1
        self.current_state = None
        self.traj = None
        self.cost = None
        self.control_steer = None
        self.control_steer_list = None
        self.control_steer_error = 0
        self.control_thr = None
        self.control_thr_list = None
        self.control_thr_error = None

    def init_mpc(self, Vehicle: VehicleParams = None, N: int = 25, dt: float = 0.1, cost: dict = None):
        if Vehicle is None:
            Vehicle = VehicleParams()
            Vehicle.load_vehicle_params()
            Vehicle.calculate_axis()
        self.Vehicle = Vehicle
        self.predicted_step = N
        self.dt = dt
        if cost is None:
            self.cost = json.loads(open("../conf/cost_config.json", "r").read())
        else:
            self.cost = cost


    def update_state(self, state, traj):
        self.current_state = state
        self.traj = traj

    def update_control(self):
        self.update_longitudinal()
        self.update_lateral()


        # lateral = threading.Thread(target=self.update_lateral)
        # lateral.setDaemon(True)
        # lateral.start()
        # longitudinal = threading.Thread(target=self.update_longitudinal)
        # longitudinal.setDaemon(True)
        # longitudinal.start()
        # lateral.join()
        # longitudinal.join()
        return self.control_steer, self.control_thr

    def update_lateral(self):
        self.lateral = ProcessedMatrix()
        tran_control = np.array([[self.control_steer if self.control_steer else 0]])
        tran_state = np.array([[float(self.current_state['y'][0])],
                               [float(self.current_state['h'][0])],
                               [self.control_steer if self.control_steer else 0]])
        self.lateral.init_process(tran_state, tran_control, self.predicted_step, vehicle_conf=self.Vehicle,
                                  v_ref=list(self.traj['v']), dt=self.dt)
        state_cost = [self.cost['lateral']['w1'], self.cost['lateral']['w2'], self.cost['lateral']['w3']]
        control_cost = [self.cost['lateral']['w4']]
        self.lateral.calculate_qn(state_cost)
        self.lateral.calculate_rn(control_cost)
        self.lateral.calculate_x_ref(self.traj)
        self.lateral.calculate_H()
        self.lateral.calculate_g()
        self.lateral.calculate_Acon()
        self.lateral.calculate_min_max(- self.Vehicle.vehicle_params["maximum_angle"],
                                       self.Vehicle.vehicle_params["maximum_angle"],
                                       state_min=[-100, -180, -self.Vehicle.vehicle_params["maximum_angle"]],
                                       state_max=[100, 180, self.Vehicle.vehicle_params["maximum_angle"]])
        self.lateral.init_qp()
        self.lateral.solove_qp()
        if self.lateral.res.info.status != 'solved':
            logger.error("Lateral control failed to solve the QP problem.")
            if (self.control_steer_list is not None and any(self.control_steer_list)
                    and self.control_steer_error < self.predicted_step):
                self.control_steer_error += 1
                if self.control_steer_error < self.predicted_step:
                    self.control_steer = float(self.control_steer_list[self.control_steer_error][0])
                else:
                    self.control_steer = 0
            else:
                self.control_steer = 0
        else:
            self.control_steer = float(self.lateral.predicted_control[0][0])
            self.control_steer_list = self.lateral.predicted_control
            self.control_steer_error = 0

    def update_longitudinal(self):
        self.longitudinal = ProcessedMatrix()
        self.longitudinal.init_process(N=self.predicted_step, vehicle_conf=self.Vehicle, dt=self.dt)
        if float(self.current_state['v'][0]) <= 0.01:
            self.control_thr = 0
        lon_state = np.array([[0],
                              [float(self.current_state['v'][0])],
                              [0]])
        lon_control = np.array([[self.control_thr if self.control_thr else 0]])
        self.longitudinal.init_process(xk=lon_state, uk=lon_control)
        state_cost = [self.cost['longitudinal']['w1'], self.cost['longitudinal']['w2'], self.cost['longitudinal']['w3']]
        control_cost = [self.cost['longitudinal']['w4']]
        self.longitudinal.calculate_qn(state_cost)
        self.longitudinal.calculate_rn(control_cost)
        self.longitudinal.calculate_x_ref_lon(self.traj)
        self.longitudinal.calculate_H()
        self.longitudinal.calculate_g()
        self.longitudinal.calculate_Acon(lat=False)
        self.longitudinal.calculate_min_max(- self.Vehicle.vehicle_params["maximum_acceleration"],
                                            self.Vehicle.vehicle_params["maximum_acceleration"],
                                            state_min=
                                            [-0.001, -0.1, -self.Vehicle.vehicle_params["maximum_acceleration"]],
                                            state_max=[1000, self.Vehicle.vehicle_params["maximum_velocity"],
                                                       self.Vehicle.vehicle_params["maximum_acceleration"]])
        self.longitudinal.update_qp()
        if self.longitudinal.res.info.status != "solved":
            print("lon_state", lon_state)
            print("lon_control", lon_control)
            logger.error("Longitudinal control failed to solve the QP problem.")
            if (self.control_thr_list is not None and any(self.control_thr_list)
                    and self.control_thr_error < self.predicted_step):
                self.control_thr_error += 1
                self.control_thr = float(self.control_thr_list[self.control_thr_error][0])
            else:
                self.control_thr = 0
        else:
            self.control_thr = float(self.longitudinal.predicted_control[0][0])
            self.control_thr_list = self.longitudinal.predicted_control
            self.control_thr_error = 0
